package aiingames.simulation.server.physics;

import aiingames.simulation.agent.AgentAction;


public class AgentMovement {

	private static final double tau = 0.5;
	
	public void updateMovement(AgentAction a, Particle p) {
		double scale = Math.sqrt(Math.pow(a.getDestination().x,2)+Math.pow(a.getDestination().y,2));
		if (scale == 0) {
			return;
		}
		double dx = a.getDestination().x/scale;
		double dy = a.getDestination().y/scale;
		
		double perc = Math.max(0, Math.min(a.getVelocity(), 1));
		double desiredVelocity = p.getMaxVelocity() * perc;
		
		double driveX = dx * desiredVelocity;
		double driveY = dy * desiredVelocity;
		
		double fx = p.getWeight() * (driveX - p.getForce().getVx())/tau;
		double fy = p.getWeight() * (driveY - p.getForce().getVy())/tau;
		
		p.getForce().incrementX(fx);
		p.getForce().incrementY(fy);
	}
	
}
